//#include <ServoSoft.h> 
 
// a maximum of eight servo objects can be created 
//Servo servoY;  // Servo que movimenta o sonar paralelamente ao solo


#define LEITURA_SONAR 's' // Obtem a leitura do sonar
#define ZERA_SERVOS 'z' // Manda todos os servos para a posicao 0 graus
#define servoY_HORARIO 'h' // Incrementa o servoY 1 grau no sentido horario
#define servoY_ANTI_HORARIO 'a' // Incrementa o servoY 1 grau no sentido anti-horario


#include "WProgram.h"
void setup();
long int leSonar(int nMedidas);
void loop();
int ledPin = 13; 
int outSonar = 5;                


void setup() 
{
  Serial.begin(9600); 

  //servoY.attach(9);  // attaches the servoY on pin 9 to the servo object 
 
  pinMode(4, OUTPUT); // sonar - GND
  pinMode(5, INPUT); // sonar - OUT  
  pinMode(6, OUTPUT); // sonar - IN
  pinMode(7, OUTPUT); // sonar - VCC
  pinMode(ledPin, OUTPUT);      
   
  
  digitalWrite(4, LOW); // sonar - GND
  digitalWrite(7, HIGH); // sonar - VCC
  digitalWrite(6, LOW); // sonar - IN
  
}


long int leSonar(int nMedidas)
{
long int  count = 0;

        for(int i=0; i<nMedidas; i++ )
        {
          while(!digitalRead(outSonar))
           {;}
          
          while(digitalRead(outSonar))  // read input value
            count++;
          
        }
        
        return count/nMedidas;
}

void loop()
{
  int i;
  long int medidaSonar;
  int servoY_Pos = 0;    // variable to store the servo position  
  
  //if( Serial.available() )
   // while(true)
    //{
      
      //switch( Serial.read() )
      //{
        //case LEITURA_SONAR :
                
          medidaSonar = leSonar(1);
          Serial.print(medidaSonar/10);
          Serial.println("cm");
          
          delay(200);        
        //break; 
   
      
       /* case ZERA_SERVOS :
        
            servoY_Pos = 30;
            servoY.write(servoY_Pos);
            delay(200);
            medidaSonar = leSonar(2);          
            Serial.println(medidaSonar);                    
            
        break;
        
        case servoY_HORARIO : // varre da eskerda para a _D_ireita
         
           if( servoY_Pos >= 0 || servoY_Pos <= 180 )
           {          
              servoY_Pos = servoY_Pos+1;
              servoY.write(servoY_Pos);
              delay(45); // tempo para o servo chegar a posicao desejada
              medidaSonar = leSonar(2);          
              Serial.println(medidaSonar);                    
           } 
         
        break;
    

        case servoY_ANTI_HORARIO : // varre da direita para a _E_squerda
        
          if( servoY_Pos >= 0 || servoY_Pos <= 180 )
           {
              servoY_Pos = servoY_Pos-1;           
              servoY.write(servoY_Pos);
              delay(45); // tempo para o servo chegar a posicao desejada
              medidaSonar = leSonar(2);          
              Serial.println(medidaSonar);                       
          }   
        
        break;*/
     
     //} 
   //}
}


int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

